﻿using PLCSharp.CoreModel.Tools;
using PLCSharp.SDK.MotionControl.EMC;
using PLCSharp.SDK.MotionControl.SMC;
using static PLCSharp.Views.MotionController.Axis;
using static PLCSharp.Views.MotionController.InterpolationGroup;

namespace PLCSharp.Views.MotionController
{
    public class EMC : ControllerSDK
    {
        public override void Close()
        {
            LTDMC.dmc_board_close();
        }

        public override bool Init(ushort controllerNo, string ip)
        {

            var done = LTDMC.dmc_board_init_eth(controllerNo, ip);
            if (done == 0)
            {
                ControllerNo = controllerNo;
                return true;
            }
            else
            {
                return false;

            }
        }

        public override void Get_Axis_Status(ushort axisNo)
        {
            while (AxisIOs.Count <= axisNo)
            {
                AxisIOs.Add(new AxisState());
            }
            var io = LTDMC.dmc_axis_io_status(ControllerNo, axisNo);
            ushort statemachine = 0;
            AxisIOs[axisNo].ALM = io.GetBit(0);
            AxisIOs[axisNo].LimitP = io.GetBit(1);
            AxisIOs[axisNo].LimitN = io.GetBit(2);
            AxisIOs[axisNo].ORG = io.GetBit(4);
            AxisIOs[axisNo].Moving = LTDMC.dmc_check_done(ControllerNo, axisNo) == 0;
            AxisIOs[axisNo].PowerOn = LTDMC.nmc_get_axis_state_machine(ControllerNo, axisNo, ref statemachine) == 4;

            double pos = 0;
            LTDMC.dmc_get_position_unit(ControllerNo, axisNo, ref pos);
            AxisIOs[axisNo].CommandPosition = pos;
            double vel = 0;
            LTDMC.dmc_read_current_speed_unit(ControllerNo, axisNo, ref vel);
            AxisIOs[axisNo].Velocity = vel;
            double encoder = 0;
            LTDMC.dmc_get_encoder_unit(ControllerNo, axisNo, ref encoder);
            AxisIOs[axisNo].EncoderPosition = encoder;

        }

        public override void GetDI()
        {

            var group = DI_Count / 32;

            if (DI_Count % 32 > 0)
            {
                group++;
            }

            for (int i = 0; i < group; i++)
            {
                var dq = LTDMC.dmc_read_inport(ControllerNo, (ushort)i);
                if (DI.Count > i)
                {
                    DI[i] = dq;
                }
                else
                {
                    DI.Add(dq);
                }
            }
        }

        public override void GetDQ()
        {

            var group = DQ_Count / 32;

            if (DQ_Count % 32 > 0)
            {
                group++;
            }

            for (int i = 0; i < group; i++)
            {
                var dq = LTDMC.dmc_read_outport(ControllerNo, (ushort)i);
                if (DQ.Count > i)
                {
                    DQ[i] = dq;
                }
                else
                {
                    DQ.Add(dq);
                }
            }
        }

        public override void SetDQ(ushort port, ushort off_on)
        {
            LTDMC.dmc_write_outbit(ControllerNo, port, off_on);
        }

        public override void Stop(ushort axisNo)
        {
            LTDMC.dmc_stop(ControllerNo, axisNo, 0);
        }

        public override void Reset(ushort axisNo)
        {


            ushort errcode = 0;
            LTDMC.nmc_get_errcode(ControllerNo, axisNo, ref errcode); //获取总线状态
            if (errcode != 0) { 
 
                LTDMC.nmc_clear_errcode(ControllerNo, axisNo);//尝试清除总线错误
                LTDMC.nmc_driver_reset(ControllerNo, axisNo);
            }

        }

        public override short CreateORG(ushort axisNo, AxisParams axisParams)
        {
            ushort errcode = 1;
            LTDMC.nmc_get_errcode(ControllerNo, axisNo, ref errcode); //获取总线状态
            if (errcode == 0) //总线正常 
            {
                // 设置回原点运动速度参数
                var ret = LTDMC.nmc_set_home_profile(ControllerNo, axisNo,
                      axisParams.HomeMode,
                      axisParams.HomeLowVelocity,
                      axisParams.HomeHighVelocity,
                      axisParams.Acc,
                      axisParams.Dec,
                     0
                      );
                if (ret != 0) { return ret; }
                return LTDMC.nmc_home_move(ControllerNo, axisNo);
            }
            else
            {
                return (short)errcode;
            }
        }

        public override short Power(ushort axisNo, bool onoff)
        {
            ushort errcode = 1;
            LTDMC.nmc_get_errcode(ControllerNo, axisNo, ref errcode); //获取总线状态
            if (errcode == 0)
                if (onoff)
                    return LTDMC.nmc_set_axis_enable(ControllerNo, axisNo);
                else
                    return LTDMC.nmc_set_axis_disable(ControllerNo, axisNo);
            else
            {
                return (short)errcode;
            }
        }

        public override bool CheckORG(ushort axisNo)
        {
            ushort state = 0;
            LTDMC.dmc_get_home_result(ControllerNo, axisNo, ref state);
            return state == 1;
        }

        public override short Move(ushort axisNo, ushort mode, AxisParams axisParams)
        {
            var maxVel = axisParams.MaxVelocity * axisParams.Rate / 100;
            var ret = LTDMC.dmc_set_profile_unit(ControllerNo, axisNo,
                    axisParams.MinVelocity,
                    maxVel,
                    axisParams.Acc,
                    axisParams.Dec,
                    axisParams.StopVelocity
                    );
            if (ret != 0) { return ret; }
            if (mode == 0)
            {
                ret = LTDMC.dmc_pmove_unit(ControllerNo, axisNo, axisParams.TargetDistance, mode);
            }
            else
            {
                ret = LTDMC.dmc_pmove_unit(ControllerNo, axisNo, axisParams.TargetPos, mode);
            }
            return ret;
        }

        public override short MulticoorMove(List<Interpolation> interpolations, InterpolationGroupParams interpolationParams)
        {
            var ret = LTDMC.dmc_set_vector_profile_unit(ControllerNo,
                interpolationParams.Coordinate,
                interpolationParams.MinVelocity,
                interpolationParams.MaxVelocity,
                interpolationParams.Acc,
                interpolationParams.Dec,
                interpolationParams.StopVelocity);
            if (ret != 0) { return ret; }
            ret = LTDMC.dmc_set_vector_s_profile(ControllerNo, interpolationParams.Coordinate, 0, 0);
            if (ret != 0) { return ret; }
            ret = LTDMC.dmc_conti_set_blend(ControllerNo, interpolationParams.Coordinate, 0);
            if (ret != 0) { return ret; }
            ret = LTDMC.dmc_stop_multicoor(ControllerNo, interpolationParams.Coordinate, 1);
            if (ret != 0) { return ret; }

            //打开连续插补缓冲区
            ret = LTDMC.dmc_conti_open_list(ControllerNo, interpolationParams.Coordinate, 2,
                [interpolationParams.AxisXNo, interpolationParams.AxisYNo]);
            if (ret != 0) { return ret; }
            //延迟输出
            ret = LTDMC.dmc_conti_delay_outbit_to_start(ControllerNo, interpolationParams.Coordinate,
                interpolationParams.OutBit,
                 0,
                 interpolationParams.DelayValue, 1, 0);
            if (ret != 0) { return ret; }
            //提前关闭输出
            ret = LTDMC.dmc_conti_ahead_outbit_to_stop(ControllerNo,
                interpolationParams.Coordinate,
                0,
                1,
                interpolationParams.Ahead,
                1,
                0);
            if (ret != 0) { return ret; }

            foreach (var item in interpolations)
            {
                switch (item.Type)
                {
                    case InterpolationType.Line直线:
                        ret = LTDMC.dmc_conti_line_unit(ControllerNo,
                            interpolationParams.Coordinate,
                            2,
                            [interpolationParams.AxisXNo, interpolationParams.AxisYNo],
                            [item.InterpolationPoints[0].X, item.InterpolationPoints[0].Y],
                            item.Params.PositionMode,
                            0);
                        break;
                    case InterpolationType.Arc圆弧:
                        ret = LTDMC.dmc_conti_arc_move_center_unit(ControllerNo,
                        interpolationParams.Coordinate, 2,
                        [interpolationParams.AxisXNo, interpolationParams.AxisYNo],
                         [item.InterpolationPoints[1].X, item.InterpolationPoints[1].Y],
                          [item.InterpolationPoints[0].X, item.InterpolationPoints[0].Y],
                         item.Params.Dir,
                         0,
                         item.Params.PositionMode,
                         0);
                        break;
                    default:
                        break;
                }

            }
            ret = LTDMC.dmc_conti_start_list(ControllerNo,
                interpolationParams.Coordinate);
            if (ret != 0) { return ret; }
            ret = LTDMC.dmc_conti_close_list(ControllerNo,
                interpolationParams.Coordinate);

            return ret;
        }

        public override short Save(ushort axisNo, AxisParams axisParams)
        {
            var ret = LTDMC.dmc_set_equiv(ControllerNo, axisNo, axisParams.Equiv);
            if (ret != 0) { return ret; }
            
            
            return ret;
        }
    }

}
